#include "Communication/SubBumper.h"
#include "data_struct/base/DataSet.h"

namespace behavior_controller
{
BumperData DataSet::bumper_data;
namespace SubBumper
{

static rclcpp::Subscription<gazebo_msgs::msg::ContactsState>::SharedPtr bumperSubSim;
static rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr bumperSub;

void sub_callback2(const std_msgs::msg::UInt8MultiArray::SharedPtr msg)
{
    DataSet::bumper_data.SetAllData(msg->data[0], msg->data[1]);
}

void sub_callback(const gazebo_msgs::msg::ContactsState::SharedPtr msg)
{
    if (msg->states.size() > 0)
        DataSet::bumper_data.SetData(DataSet::bumper_data.bumper_map[msg->header.frame_id], 1);
    else
        DataSet::bumper_data.SetData(DataSet::bumper_data.bumper_map[msg->header.frame_id], 0);
}

void SubBumperInfo(rclcpp::Node::SharedPtr &node_handle)
{
    if (SIM_MODE == 0)
    {
        bumperSub = node_handle->create_subscription<std_msgs::msg::UInt8MultiArray>(
                "bumper", 2, sub_callback2);
    } else
    {
        bumperSubSim = node_handle->create_subscription<gazebo_msgs::msg::ContactsState>(
                "bumper", 6, sub_callback);
    }
    std::cout << "SubBumper SubBumperInfo" << std::endl;
}
} // namespace SubBumper

} // namespace behavior_controller